#ifndef __UDSF_BBE_C__
#define __UDSF_BBE_C__

#include "app.h"
/*
*   notice:
*   UDSF 同UDF编程：不可调用cstd-lib,只可调用"sdk_ifs_udk_cfg.h"、"sys_api.h"中列出的API函数。
*   UDSF文件内函数均需使用static修饰符，且UDF中以#include "udsf_xxx.c"形式引用
*/

static void udsf_memcpy32_hw(uint32_t*src,uint32_t*dst,int len){ // notice: src or dst addr must be in abufx;and src and dst must be in diffrent abufx。

	P2_FFT_CONFIG p2_cfg;

	//intraCnt,fix2pfLsfB,max_clr,p2_fft_mode,dma_dsize,rsfBit,sub_mode,mode
	p2_cfg.p2Cfg0 = (len - 1) << 20 | P2_FIX2PF_LSFB_0BIT << 17 | P2_MAX_CLR_EN << 16 | P2_FFT_MODE_DIS << 14 | P2_DMA_DSIZE_32BIT << 12 | P2_RSF_DIV1 << 8 | P2_SUBMODE_0 << 4 |P2_MODE_DMA;
	p2_cfg.p2Cfg1 = 0,
	p2_cfg.p2Cfg2 = 0,
	p2_cfg.p2Cfg3 = 0,
	p2_cfg.p2Cfg4 = (NUM_BYTE32) << 16 | (GET_OFFSET((uint32_t)src)) << 0, // covered by udf03
	p2_cfg.p2Cfg5 = 0,
	p2_cfg.p2Cfg6 = 0,
	p2_cfg.p2Cfg7 = 0,
	p2_cfg.p2Cfg8 = (NUM_BYTE32) << 16 | (GET_OFFSET((uint32_t)dst)) << 0, // covered by udf03

	P2_FFT_HANDLE(&p2_cfg);
}

static void udsf_rangeRefinement(pc_rst_t * pt,uint8_t range_inter_fac,uint32_t*pABS) __attribute__((unused));
static void udsf_rangeRefinement2(pc_rst_t * pt,uint8_t range_inter_fac,uint32_t*pABS) __attribute__((unused));

static void udsf_rangeRefinement(pc_rst_t * pt,uint8_t range_inner_factor,uint32_t*pABS)
{
    uint16_t d1Idx, d1LastIdx, d1NextIdx;
    uint8_t d2Idx;
    uint32_t d1CurrPow,d1LastPow, d1NextPow;
    int32_t valTmp1, valTmp2, valTmp3;
	
    if (pt->targetNum > 0)
    {
        for (uint16_t n = 0; n < pt->targetNum; n++)
        {
            d1Idx = pt->target[n].d1Idx;
            d2Idx = pt->target[n].d2Idx;
            d1LastIdx = ((d1Idx - 1) < 1) ? 2 : (d1Idx - 1);
            d1NextIdx = ((d1Idx + 1) >= UDFDEF_RANGE_USE) ? (UDFDEF_RANGE_USE - 1) : (d1Idx + 1);
            d1LastPow = pABS[d2Idx * UDFDEF_RANGE_USE + d1LastIdx];
            d1NextPow = pABS[d2Idx * UDFDEF_RANGE_USE + d1NextIdx];
            d1CurrPow = pABS[d2Idx * UDFDEF_RANGE_USE + d1Idx];
            if (((d1CurrPow >= d1LastPow) && (d1CurrPow > d1NextPow)) || ((d1CurrPow > d1LastPow) && (d1CurrPow >= d1NextPow)))
            {
                valTmp1 = (d1NextPow - d1LastPow) >> 1;
                valTmp1 = valTmp1 << range_inner_factor;
                valTmp2 = 2 * d1CurrPow - d1NextIdx - d1LastPow;
                valTmp3 = valTmp1 / valTmp2;
                pt->target[n].d1Idx = d1Idx * range_inner_factor + valTmp3;
            }
            else
            {
                pt->target[n].d1Idx = d1Idx * range_inner_factor;
            }
        }
    }
}

static void udsf_rangeRefinement2(pc_rst_t * pt,uint8_t range_inner_factor,uint32_t*pABS)
{
	for( int i=0;i<pt->targetNum;i++ ){
		
		uint32_t range_start,range_end;
		
		range_start = pt->target[i].d1Idx * range_inner_factor;
		pt->target[i].d1Idx = range_start;
		//LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,&range_start,1,"\nOrigin:","\n");
		
		if( range_start >= range_inner_factor ){
				range_end = range_start+1;
				range_start -= (range_inner_factor-1);
		}else{
			range_start = 0;
			range_end = (range_inner_factor-1);
		}

		//LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,&range_start,1,"\nstart:","\n");
		//LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,&range_end,1,"\nend:","\n");
		uint32_t max = 0;
		for( int j=range_start;j<range_end;j++ ){
			if( pABS[j] > max ){
				max = pABS[j];
				//LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,&j,1,"\nj:","\n");
				pt->target[i].d1Idx = j;
			}
		}
		
		//LOG_SEND_U16_BASE16(LOG_LEVEL_EMERG,1,&pt->target[i].d1Idx,1,"\nabs max:","\n");
	}
}

static void udsf_bbe_p2_row_mul_coef(uint16_t row_len,uint32_t src0_addr,uint32_t coef_addr,uint32_t dst_addr)
{
//	uint32_t bkup = *(uint32_t*)src1_addr;
//	*(uint32_t*)src1_addr = coef;

	#if SDK_VERSION < SDK_VERSION_GENE(3,0)
	P2_FFT_CONFIG_NEW cfg;
	#else
	P2_FFT_CONFIG cfg;
	#endif
	
	cfg.p2Cfg0=(row_len-1)<< 20 |\
							P2_FIX2PF_LSFB_0BIT<<17 |\
							P2_MAX_CLR_EN<<16       |\
							P2_FFT_MODE_DIS<<14      |\
							P2_DMA_DSIZE_32BIT<<12   |\
							P2_RSF_DIV1<<8          |\
							P2_SUBMODE_0<<4         |\
							P2_MODE_MUL;

	//jumpCnt,interCnt
	cfg.p2Cfg1=(0)<< 12 |(0)<< 0;
	//src0IntraInc,src0BaseAddr	
	cfg.p2Cfg4=(NUM_BYTE32)<<16 |(GET_OFFSET(src0_addr))<< 0;
	//src1IntraInc,src1BaseAddr  
	cfg.p2Cfg6=(0)<<16 |(GET_OFFSET(coef_addr))<< 0;
	//dstIntraInc,dstBaseAddr   			
	cfg.p2Cfg8=(NUM_BYTE32)<<16 |(GET_OFFSET(dst_addr))<<0;
	P2_FFT_HANDLE((P2_FFT_CONFIG*)&cfg);
	//*(uint32_t*)src1_addr = bkup;
}

static void udsf_bbe_p2_row_abs_add(uint16_t row_len,uint32_t row_a_addr,uint32_t row_b_addr,uint32_t dst_addr)
{
	#if SDK_VERSION < SDK_VERSION_GENE(3,0)
	P2_FFT_CONFIG_NEW cfg;
	#else
	P2_FFT_CONFIG cfg;
	#endif
	
	cfg.p2Cfg0=(2-1)<< 20 |\
							P2_FIX2PF_LSFB_0BIT<<17 |\
							P2_MAX_CLR_EN<<16       |\
							P2_FFT_MODE_DIS<<14      |\
							P2_DMA_DSIZE_32BIT<<12   |\
							P2_RSF_DIV1<<8          |\
							P2_SUBMODE_1<<4         |\
							P2_MODE_ACC;

	//jumpCnt,interCnt
	cfg.p2Cfg1=(0)<< 12 |(row_len-1)<< 0;
	//src0JumpInc,src0InterInc
	cfg.p2Cfg3=(0)<<16 |(NUM_BYTE32)<< 0;
	//src0IntraInc,src0BaseAddr	
	cfg.p2Cfg4=(row_b_addr-row_a_addr)<<16 |(GET_OFFSET(row_a_addr))<< 0;
	//dstJumpInc,dstInterInc  
	cfg.p2Cfg7=(0)<<16 |(NUM_BYTE32)<< 0;
	//dstIntraInc,dstBaseAddr   			
	cfg.p2Cfg8=(0)<<16 |(GET_OFFSET(dst_addr))<<0;
	
	P2_FFT_HANDLE((P2_FFT_CONFIG*)&cfg);
}

static int udsf_bbe_cfar_row_test(uint32_t cfar_src,uint32_t cfar_dst,uint8_t cfar_mode,uint8_t work_mode,
uint8_t intra_cnt,uint16_t intra_inc,
uint8_t guard_size,uint8_t search_size,uint8_t th,
uint32_t cutThres
){
	#if SDK_VERSION < SDK_VERSION_GENE(3,0)
	CFAR_CONFIG_NEW cfar_cfg;
	#else
	CFAR_CONFIG cfar_cfg;
	#endif

	cfar_cfg.cfarCfg0 = (CFAR_PEAKDET_DIS) << 8 | (cfar_mode) << 4 | (work_mode) << 0;
	//interCnt,intraCnt
	cfar_cfg.cfarCfg1 = (1) << 12 | (intra_cnt) << 0;
	// wrapDirA,wrapDirB,thresDiv,divFac,mulFac,searchSize,guardSize
	cfar_cfg.cfarCfg2 = (1) << 25 | (1) << 24 | (CFAR_THRESDIV4) << 20 | (CFAR_DIVFAC1) << 16 | (th) << 8 | (search_size) << 4 | (guard_size) << 0;
	// cutThres
	cfar_cfg.cfarCfg3 = (cutThres) << 0;
	// targetLim
	cfar_cfg.cfarCfg4 = (CFAR_MAX_NUM - 1) << 0;
	// srcInterInc
	cfar_cfg.cfarCfg5 = (/*inter_inc*/0) << 0;
	// srcIntraInc,srcBaseAddr
	cfar_cfg.cfarCfg6 = (intra_inc) << 16 | (GET_OFFSET(cfar_src)) << 0;
	// wrapAddrA,wrapAddrB
	cfar_cfg.cfarCfg7 = (uint32_t)(GET_OFFSET(cfar_src) + (search_size + guard_size)*intra_inc) << 16 | (GET_OFFSET(cfar_src)+(intra_cnt-2)*intra_inc) << 0;
	// dstBaseAddr
	cfar_cfg.cfarCfg8 = (GET_OFFSET(cfar_dst)) << 0;
	cfar_cfg.cfarAbsAddr = cfar_src+BBE_ABS_OFFSET;
	
	uint32_t cfarRstNum = CFAR_HANDLE((CFAR_CONFIG*)&cfar_cfg);

	return cfarRstNum;
}

#endif
